import * as THREE from 'three';
import { STLLoader } from 'three/examples/jsm/loaders/STLLoader.js';
import { ColladaLoader } from 'three/examples/jsm/loaders/ColladaLoader.js';
import { URDFRobot, URDFJoint, URDFLink, URDFCollider, URDFVisual, URDFMimicJoint } from './URDFClasses.js';

/*
THREE.js和ROS的参考坐标系。
两个坐标系都是右手坐标系，因此URDF在没有右手坐标系的情况下被实例化
帧变换。可以旋转得到的模型以校正正确的向上，
向右和向前的方向

THREE.js
   Y
   |
   |
   .-----X
 ／
Z

ROS URDf
       Z
       |   X
       | ／
 Y-----.

*/

const tempQuaternion = new THREE.Quaternion();
const tempEuler = new THREE.Euler();

// 取一个向量“x y z”并将其处理为
// 数组[x，y，z]
function processTuple(val) {

  if (!val) return [0, 0, 0];
  return val.trim().split(/\s+/g).map(num => parseFloat(num));

}

// 以URDF顺序对threejs对象应用旋转
function applyRotation(obj, rpy, additive = false) {

  // 如果添加剂为真，则应用旋转
  // 除了现有的轮换
  if (!additive) obj.rotation.set(0, 0, 0);

  tempEuler.set(rpy[0], rpy[1], rpy[2], 'ZYX');
  tempQuaternion.setFromEuler(tempEuler);
  tempQuaternion.multiply(obj.quaternion);
  obj.quaternion.copy(tempQuaternion);

}

/* URDFLoader Class */
// 将URDF文件加载并读取为THREEjs Object3D格式
export default
  class URDFLoader {
  constructor(manager) {
    this.manager = manager || THREE.DefaultLoadingManager;
    this.loadMeshCb = this.defaultMeshLoader.bind(this);
    this.parseVisual = true;
    this.parseCollision = false;
    this.packages = '';
    this.workingPath = '';
    this.fetchOptions = {};
  }

  /* Public API */
  loadAsync(urdf) {

    return new Promise((resolve, reject) => {

      this.load(urdf, resolve, null, reject);

    });

  }

  // urdf：包内urdf的路径或绝对值
  // onComplete：加载后传递给模型的回调
  load(urdf, onComplete, onProgress, onError) {
    // 检查之前是否指定了完整的URI
    // 添加包裹信息
    const manager = this.manager;
    const workingPath = THREE.LoaderUtils.extractUrlBase(urdf);
    const urdfPath = this.manager.resolveURL(urdf);

    manager.itemStart(urdfPath);

    fetch(urdfPath, this.fetchOptions)
      .then(res => {
        if (res.ok) {
          if (onProgress) {
            onProgress(null);
          }
          return res.text();
        } else {
          throw new Error(`URDFLoader: Failed to load url '${urdfPath}' with error code ${res.status} : ${res.statusText}.`);
        }

      })
      .then(data => {
        if (this.workingPath === '') {
          this.workingPath = workingPath;
        }
        const model = this.parse(data);

        onComplete(model);
        manager.itemEnd(urdfPath);
      })
      .catch(e => {
        if (onError) {
          onError(e);
        } else {
          console.error('URDFLoader: Error loading file.', e);
        }
        manager.itemError(urdfPath);
        manager.itemEnd(urdfPath);
      });

  }

  parse(content) {

    const packages = this.packages;
    const loadMeshCb = this.loadMeshCb;
    console.log(this, 'this');

    const parseVisual = this.parseVisual;
    const parseCollision = this.parseCollision;
    const workingPath = this.workingPath;
    const manager = this.manager;
    const linkMap = {};
    const jointMap = {};
    const materialMap = {};

    // 解析网格文件的路径
    function resolvePath(path) {

      if (!/^package:\/\//.test(path)) {

        return workingPath ? workingPath + path : path;

      }

      // 删除“package://”关键字，并在第一个斜线处拆分meshPath
      const [targetPkg, relPath] = path.replace(/^package:\/\//, '').split(/\/(.+)/);

      if (typeof packages === 'string') {

        // “pkg”是一个单独的包
        if (packages.endsWith(targetPkg)) {

          // “pkg”是目标包
          return packages + '/' + relPath;

        } else {

          // 假设“pkg”是目标包的父目录
          return packages + '/' + targetPkg + '/' + relPath;

        }

      } else if (packages instanceof Function) {

        return packages(targetPkg) + '/' + relPath;

      } else if (typeof packages === 'object') {

        // “pkg”是包的映射
        if (targetPkg in packages) {

          return packages[targetPkg] + '/' + relPath;

        } else {

          console.error(`URDFLoader : ${targetPkg} not found in provided package list.`);
          return null;

        }

      }

    }

    // 处理URDF文本格式
    function processUrdf(data) {

      let children;
      if (data instanceof Document) {

        children = [...data.children];

      } else if (data instanceof Element) {

        children = [data];

      } else {

        const parser = new DOMParser();
        const urdf = parser.parseFromString(data, 'text/xml');
        children = [...urdf.children];

      }

      const robotNode = children.filter(c => c.nodeName === 'robot').pop();
      return processRobot(robotNode);

    }

    // 处理<robot>节点
    function processRobot(robot) {

      const robotNodes = [...robot.children];
      const links = robotNodes.filter(c => c.nodeName.toLowerCase() === 'link');
      const joints = robotNodes.filter(c => c.nodeName.toLowerCase() === 'joint');
      const materials = robotNodes.filter(c => c.nodeName.toLowerCase() === 'material');
      const obj = new URDFRobot();

      obj.robotName = robot.getAttribute('name');
      obj.urdfRobotNode = robot;

      // 创建<material>贴图
      materials.forEach(m => {
        const name = m.getAttribute('name');
        materialMap[name] = processMaterial(m);
      });

      // 创建<link>地图
      const visualMap = {};
      const colliderMap = {};
      links.forEach(l => {
        const name = l.getAttribute('name');
        const isRoot = robot.querySelector(`child[link="${name}"]`) === null;
        linkMap[name] = processLink(l, visualMap, colliderMap, isRoot ? obj : null);
      });

      // 创建<join>贴图
      joints.forEach(j => {
        const name = j.getAttribute('name');
        jointMap[name] = processJoint(j);
      });

      obj.joints = jointMap;
      obj.links = linkMap;
      obj.colliders = colliderMap;
      obj.visual = visualMap;

      // 连接模拟关节
      const jointList = Object.values(jointMap);
      jointList.forEach(j => {
        if (j instanceof URDFMimicJoint) {
          jointMap[j.mimicJoint].mimicJoints.push(j);
        }
      });

      // 检测模拟关节的无限循环
      jointList.forEach(j => {

        const uniqueJoints = new Set();
        const iterFunction = joint => {

          if (uniqueJoints.has(joint)) {

            throw new Error('URDFLoader: Detected an infinite loop of mimic joints.');

          }

          uniqueJoints.add(joint);
          joint.mimicJoints.forEach(j => {

            iterFunction(j);

          });

        };

        iterFunction(j);
      });

      obj.frames = {
        ...colliderMap,
        ...visualMap,
        ...linkMap,
        ...jointMap,
      };

      return obj;

    }

    // 处理关节节点并将其作为父节点
    function processJoint(joint) {
      const children = [...joint.children];
      const jointType = joint.getAttribute('type');
      let obj;
      const mimicTag = children.find(n => n.nodeName.toLowerCase() === 'mimic');
      if (mimicTag) {
        obj = new URDFMimicJoint();
        obj.mimicJoint = mimicTag.getAttribute('joint');
        obj.multiplier = parseFloat(mimicTag.getAttribute('multiplier') || 1.0);
        obj.offset = parseFloat(mimicTag.getAttribute('offset') || 0.0);
      } else {
        obj = new URDFJoint();

      }

      obj.urdfNode = joint;
      obj.name = joint.getAttribute('name');
      obj.urdfName = obj.name;
      obj.jointType = jointType;

      let parent = null;
      let child = null;
      let xyz = [0, 0, 0];
      let rpy = [0, 0, 0];

      // 提取属性
      children.forEach(n => {

        const type = n.nodeName.toLowerCase();
        if (type === 'origin') {

          xyz = processTuple(n.getAttribute('xyz'));
          rpy = processTuple(n.getAttribute('rpy'));

        } else if (type === 'child') {

          child = linkMap[n.getAttribute('link')];

        } else if (type === 'parent') {

          parent = linkMap[n.getAttribute('link')];

        } else if (type === 'limit') {

          obj.limit.lower = parseFloat(n.getAttribute('lower') || obj.limit.lower);
          obj.limit.upper = parseFloat(n.getAttribute('upper') || obj.limit.upper);

        }
      });

      // 加入链接
      parent.add(obj);
      obj.add(child);
      applyRotation(obj, rpy);
      obj.position.set(xyz[0], xyz[1], xyz[2]);

      // 设置旋转功能
      const axisNode = children.filter(n => n.nodeName.toLowerCase() === 'axis')[0];

      if (axisNode) {

        const axisXYZ = axisNode.getAttribute('xyz').split(/\s+/g).map(num => parseFloat(num));
        obj.axis = new THREE.Vector3(axisXYZ[0], axisXYZ[1], axisXYZ[2]);
        obj.axis.normalize();

      }

      return obj;

    }

    // 处理<link>节点
    function processLink(link, visualMap, colliderMap, target = null) {

      if (target === null) {

        target = new URDFLink();

      }

      const children = [...link.children];
      target.name = link.getAttribute('name');
      target.urdfName = target.name;
      target.urdfNode = link;

      if (parseVisual) {

        const visualNodes = children.filter(n => n.nodeName.toLowerCase() === 'visual');
        visualNodes.forEach(vn => {

          const v = processLinkElement(vn, materialMap);
          target.add(v);

          if (vn.hasAttribute('name')) {

            const name = vn.getAttribute('name');
            v.name = name;
            v.urdfName = name;
            visualMap[name] = v;

          }

        });

      }

      if (parseCollision) {

        const collisionNodes = children.filter(n => n.nodeName.toLowerCase() === 'collision');
        collisionNodes.forEach(cn => {

          const c = processLinkElement(cn);
          target.add(c);

          if (cn.hasAttribute('name')) {

            const name = cn.getAttribute('name');
            c.name = name;
            c.urdfName = name;
            colliderMap[name] = c;

          }

        });

      }

      return target;

    }

    function processMaterial(node) {

      const matNodes = [...node.children];
      const material = new THREE.MeshPhongMaterial();

      material.name = node.getAttribute('name') || '';
      matNodes.forEach(n => {

        const type = n.nodeName.toLowerCase();
        if (type === 'color') {

          const rgba =
            n
              .getAttribute('rgba')
              .split(/\s/g)
              .map(v => parseFloat(v));

          material.color.setRGB(rgba[0], rgba[1], rgba[2]);
          material.opacity = rgba[3];
          material.transparent = rgba[3] < 1;
          material.depthWrite = !material.transparent;

        } else if (type === 'texture') {

          // URDF规范不要求<texture/>标签包括
          // 文件名属性，因此如果没有提供，则跳过加载纹理。
          const filename = n.getAttribute('filename');
          if (filename) {

            const loader = new THREE.TextureLoader(manager);
            const filePath = resolvePath(filename);
            material.map = loader.load(filePath);
            material.map.colorSpace = THREE.SRGBColorSpace;

          }

        }
      });

      return material;

    }

    // 将视觉和碰撞节点处理到网格中
    function processLinkElement(vn, materialMap = {}) {

      const isCollisionNode = vn.nodeName.toLowerCase() === 'collision';
      const children = [...vn.children];
      let material = null;

      // 先拿到材料
      const materialNode = children.filter(n => n.nodeName.toLowerCase() === 'material')[0];
      if (materialNode) {

        const name = materialNode.getAttribute('name');
        if (name && name in materialMap) {

          material = materialMap[name];

        } else {

          material = processMaterial(materialNode);

        }

      } else {
        material = new THREE.MeshPhongMaterial();
      }

      const group = isCollisionNode ? new URDFCollider() : new URDFVisual();
      group.urdfNode = vn;

      children.forEach(n => {

        const type = n.nodeName.toLowerCase();
        if (type === 'geometry') {

          const geoType = n.children[0].nodeName.toLowerCase();
          if (geoType === 'mesh') {

            const filename = n.children[0].getAttribute('filename');
            const filePath = resolvePath(filename);

            // 如果未提供包目录，则文件路径为空。
            if (filePath !== null) {

              const scaleAttr = n.children[0].getAttribute('scale');
              if (scaleAttr) {
                const scale = processTuple(scaleAttr);
                group.scale.set(scale[0], scale[1], scale[2]);

              }

              loadMeshCb(filePath, manager, (obj, err) => {

                if (err) {
                  console.error('URDFLoader: Error loading mesh.', err);
                } else if (obj) {
                  if (obj instanceof THREE.Mesh) {
                    obj.material = material;
                  }
                  // 我们不期望非身份轮换或职位。假使
                  // COLLADA文件中，模型可能附带了单位的自定义比例
                  // 转换。
                  obj.position.set(0, 0, 0);
                  obj.quaternion.identity();
                  group.add(obj);
                }
              });

            }

          } else if (geoType === 'box') {

            const primitiveModel = new THREE.Mesh();
            primitiveModel.geometry = new THREE.BoxGeometry(1, 1, 1);
            primitiveModel.material = material;

            const size = processTuple(n.children[0].getAttribute('size'));
            primitiveModel.scale.set(size[0], size[1], size[2]);

            group.add(primitiveModel);

          } else if (geoType === 'sphere') {

            const primitiveModel = new THREE.Mesh();
            primitiveModel.geometry = new THREE.SphereGeometry(1, 30, 30);
            primitiveModel.material = material;

            const radius = parseFloat(n.children[0].getAttribute('radius')) || 0;
            primitiveModel.scale.set(radius, radius, radius);

            group.add(primitiveModel);

          } else if (geoType === 'cylinder') {

            const primitiveModel = new THREE.Mesh();
            primitiveModel.geometry = new THREE.CylinderGeometry(1, 1, 1, 30);
            primitiveModel.material = material;

            const radius = parseFloat(n.children[0].getAttribute('radius')) || 0;
            const length = parseFloat(n.children[0].getAttribute('length')) || 0;
            primitiveModel.scale.set(radius, length, radius);
            primitiveModel.rotation.set(Math.PI / 2, 0, 0);

            group.add(primitiveModel);

          }

        } else if (type === 'origin') {

          const xyz = processTuple(n.getAttribute('xyz'));
          const rpy = processTuple(n.getAttribute('rpy'));

          group.position.set(xyz[0], xyz[1], xyz[2]);
          group.rotation.set(0, 0, 0);
          applyRotation(group, rpy);

        }

      });

      return group;

    }

    return processUrdf(content);
  }



};

// 默认网格加载功能
const defaultMeshLoader = (path, manager, done) => {

  if (/\.stl$/i.test(path)) {

    const loader = new STLLoader(manager);
    loader.load(path, geom => {
      const mesh = new THREE.Mesh(geom, new THREE.MeshPhongMaterial());
      done(mesh);
    });

  } else if (/\.dae$/i.test(path)) {

    const loader = new ColladaLoader(manager);
    loader.load(path, dae => done(dae.scene));

  } else {

    console.warn(`URDFLoader: Could not load model at ${path}.\nNo loader available`);

  }

}